import numpy as np
from scipy.optimize import fsolve

# 定义常量
g = 9.8  # 重力加速度 (m/s^2)
v_missile = 300  # 导弹速度 (m/s)
v_drone = 120  # 无人机速度 (m/s)
v_cloud_sink = 3  # 云团下沉速度 (m/s)
cloud_radius = 10  # 云团有效半径 (m)
max_shield_time = 20  # 云团最大有效时间 (s)

# 初始位置 (m)
target_dummy = np.array([0, 0, 0])  # 假目标
missile_M1 = np.array([20000, 0, 2000])  # 导弹M1初始位置
drone_FY1 = np.array([17800, 0, 1800])  # 无人机FY1初始位置

# 时间参数 (s)
task_receive_time = 0  # 受领任务时间
drop_delay = 1.5  # 投放延迟
burst_delay = 3.6  # 起爆延迟

# 计算导弹方向向量 (指向假目标)
missile_direction = target_dummy - missile_M1
missile_direction = missile_direction / np.linalg.norm(missile_direction)

# 计算无人机方向向量 (指向假目标)
drone_direction = target_dummy - drone_FY1
drone_direction = drone_direction / np.linalg.norm(drone_direction)

# 计算投放点位置
drop_time = task_receive_time + drop_delay
drop_position = drone_FY1 + drone_direction * v_drone * drop_delay

# 计算起爆时间
burst_time = drop_time + burst_delay

# 计算干扰弹从投放到起爆的运动
# 水平方向速度 (无人机速度方向)
v_horizontal = drone_direction * v_drone

# 垂直方向初速度 (自由落体)
v_vertical_initial = 0

# 计算起爆点位置
# 水平位移
horizontal_displacement = v_horizontal * burst_delay

# 垂直位移 (考虑重力)
vertical_displacement = v_vertical_initial * burst_delay - 0.5 * g * burst_delay ** 2

# 起爆点位置
burst_position = drop_position + np.array(
    [horizontal_displacement[0], horizontal_displacement[1], vertical_displacement])

print(f"投放点位置: {drop_position}")
print(f"起爆点位置: {burst_position}")


# 定义导弹位置函数
def missile_position(t):
    # t是从受领任务开始的时间
    return missile_M1 + missile_direction * v_missile * t


# 定义云团中心位置函数
def cloud_position(t):
    # t是从受领任务开始的时间
    if t < burst_time:
        return None  # 云团尚未形成

    # 云团形成后的时间
    cloud_time = t - burst_time

    # 水平位置不变
    x = burst_position[0]
    y = burst_position[1]

    # 垂直位置随时间下沉
    z = burst_position[2] - v_cloud_sink * cloud_time

    return np.array([x, y, z])


# 定义导弹与云团距离函数
def distance_missile_cloud(t):
    missile_pos = missile_position(t)
    cloud_pos = cloud_position(t)

    if cloud_pos is None:
        return float('inf')  # 云团尚未形成，距离设为无穷大

    return np.linalg.norm(missile_pos - cloud_pos)


# 计算导弹到达假目标的时间
distance_to_target = np.linalg.norm(missile_M1 - target_dummy)
time_to_target = distance_to_target / v_missile
print(f"导弹到达假目标时间: {time_to_target:.2f} s")

# 计算云团有效时间范围
cloud_start_time = burst_time
cloud_end_time = burst_time + max_shield_time
print(f"云团有效时间范围: {cloud_start_time:.2f} s 到 {cloud_end_time:.2f} s")

# 计算导弹在云团有效时间范围内的位置
t_values = np.linspace(cloud_start_time, min(cloud_end_time, time_to_target), 100)
distances = [distance_missile_cloud(t) for t in t_values]

# 找到最小距离
min_distance = min(distances)
min_distance_time = t_values[np.argmin(distances)]
print(f"最小距离: {min_distance:.2f} m, 发生在时间: {min_distance_time:.2f} s")

# 如果最小距离大于10m，则导弹没有进入云团
if min_distance > cloud_radius:
    print("导弹未进入云团有效范围")
    effective_time = 0
else:
    # 使用数值方法求解导弹进入和离开云团的时间
    def equation(t):
        return distance_missile_cloud(t) - cloud_radius


    try:
        # 寻找导弹进入云团的时间
        t_enter = fsolve(equation, min_distance_time - 0.1)[0]

        # 寻找导弹离开云团的时间
        t_exit = fsolve(equation, min_distance_time + 0.1)[0]

        # 确保时间在有效范围内
        t_enter = max(t_enter, cloud_start_time)
        t_exit = min(t_exit, cloud_end_time)

        # 计算有效遮蔽时间
        effective_time = t_exit - t_enter

        print(f"导弹进入云团时间: {t_enter:.6f} s")
        print(f"导弹离开云团时间: {t_exit:.6f} s")
        print(f"有效遮蔽时长: {effective_time:.6f} s")
    except:
        print("求解失败，导弹未进入云团有效范围")
        effective_time = 0

# 输出结果
result = {
    "effective_shielding_time": effective_time
}

print("\n最终结果:")
print(result)